
/*
 * Filename: dtovcfg.c
 * Description: Parse config.txt file and apply device tree overlays
 *
 * Organization: lckfb
 * Author: Caicheng Wu
 * Email: 1378913492@qq.com
 * Date: 2024-08-30
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include <common.h>
#include <command.h>
#include <linux/ctype.h>
#include <linux/types.h>
#include <asm/global_data.h>
#include <linux/libfdt.h>
#include <fdt_support.h>
#include <mapmem.h>
#include <asm/io.h>
#include <fs.h>
#include <malloc.h>

/*Add by Caicheng Wu,Start support debug*/
#define __WUDBG__  
#ifdef __WUDBG__ 
#define WUDBG(format,...) printf("WUDBG[%s,%d] "format"\n",__func__,__LINE__, ##__VA_ARGS__)    
#else  
#define WUDBG(format,...)  
#endif   
/*Add by Caicheng Wu,End support debug*/

#define MAX_SET_PARAMS 100
#define MAX_NODES 50
#define MAX_DTOVERLAYS 10
#define MAX_RM_TARGETS 50

typedef struct {
    char *path;
    char *prop;
    char *value;
} set_param_t;

typedef struct {
    char *path;
    char *node;
} node_t;

typedef struct {
    char *path;
    char *prop;
} rm_target_t;

typedef struct {
    char *overlay_name;
} dt_overlay_t;

typedef struct {
    char *name;
    char *value;
} setenv_param_t;

typedef struct {
    set_param_t set_params[MAX_SET_PARAMS];
    int set_param_count;
    
    node_t nodes[MAX_NODES];
    int node_count;
    
    rm_target_t rm_targets[MAX_RM_TARGETS];
    int rm_count;
    
    dt_overlay_t dt_overlays[MAX_DTOVERLAYS];
    int dt_overlay_count;

    setenv_param_t setenv_params[MAX_SET_PARAMS];
    int setenv_count;
} config_data_t;


/* 
 * Trim leading and trailing whitespace 
 */
static char *trim_whitespace(char *str) {
    char *end;

    while (isspace((unsigned char)*str)) str++;

    if (*str == 0)
        return str;

    end = str + strlen(str) - 1;
    while (end > str && isspace((unsigned char)*end)) end--;

    *(end + 1) = '\0';

    return str;
}

/*
 * Parse configuration file 
 */
static void parse_config(char *content, size_t size, config_data_t *config_data) {
    char *line_start = content;
    char *line_end;

    config_data->set_param_count = 0;
    config_data->node_count = 0;
    config_data->rm_count = 0;
    config_data->dt_overlay_count = 0;
    config_data->setenv_count = 0;
    
    while (line_start < content + size) {
        line_end = strchr(line_start, '\n');
        if (!line_end) {
            line_end = content + size;
        }
        *line_end = '\0';

        char *trimmed_line = trim_whitespace(line_start);

        if (*trimmed_line == '\0' || *trimmed_line == '#') {
            line_start = line_end + 1;
            continue;
        }

        if (strncmp(trimmed_line, "setenv", 6) == 0) {
            char *name = strtok(trimmed_line + 7, " ");
            char *value = strtok(NULL, "");

            config_data->setenv_params[config_data->setenv_count].name = name;
            config_data->setenv_params[config_data->setenv_count].value = value ? value : NULL;
            config_data->setenv_count++;
        }
        else if (strncmp(trimmed_line, "dtoverlay", 9) == 0) {
            config_data->dt_overlays[config_data->dt_overlay_count].overlay_name = strdup(trimmed_line + 10);
            config_data->dt_overlay_count++;
        }
        else if (strncmp(trimmed_line, "fdtset", 6) == 0) {
            char *path = strtok(trimmed_line + 7, " ");
            char *prop = strtok(NULL, " ");
            char *value = strtok(NULL, "");

            config_data->set_params[config_data->set_param_count].path = path;
            config_data->set_params[config_data->set_param_count].prop = prop;
            config_data->set_params[config_data->set_param_count].value = value;
            config_data->set_param_count++;
        }
        else if (strncmp(trimmed_line, "fdtmknode", 9) == 0) {
            char *path = strtok(trimmed_line + 10, " ");
            char *node = strtok(NULL, "");

            config_data->nodes[config_data->node_count].path = path;
            config_data->nodes[config_data->node_count].node = node;
            config_data->node_count++;
        }
        else if (strncmp(trimmed_line, "fdtrm", 5) == 0) {
            char *path = strtok(trimmed_line + 6, " ");
            char *prop = strtok(NULL, "");

            config_data->rm_targets[config_data->rm_count].path = path;
            config_data->rm_targets[config_data->rm_count].prop = prop;
            config_data->rm_count++;
        }

        line_start = line_end + 1;
    }
}

/*
 * Parse property values for device tree 
 */
static int fdt_parse_prop(char *newval, char *data, int *len) {
    char *cp;              
    unsigned long tmp; 

    *len = 0;
    
    if (*newval == '<') {
        newval++;
        while (*newval && (*newval != '>')) {
            if (*newval == '\0') {
                return 1;
            }

            cp = newval;
            tmp = simple_strtoul(cp, &newval, 0);

            *(fdt32_t *)data = cpu_to_fdt32(tmp);
            data += 4;
            *len += 4;

            while (*newval == ' ')
                newval++;
        }

        if (*newval != '>') {
            WUDBG("Unexpected character '%c'", *newval);
            return 1;
        }
    } else if (*newval == '[') {
        newval++;
        while (*newval && (*newval != ']')) {
            while (*newval == ' ')
                newval++;
            if (*newval == '\0') {
                return 1;
            }
            if (!isxdigit(*newval)) break;

            tmp = simple_strtoul(newval, &newval, 16);
            *data++ = tmp & 0xFF;
            *len += 1;
        }
        if (*newval != ']') {
            WUDBG("Unexpected character '%c'", *newval);
            return 1;
        }
    } else {
        size_t length = strlen(newval) + 1;
        strcpy(data, newval);
        *len = length;
    }

    return 0;
}

/*
 * Command to parse configuration and apply changes to device tree 
 */
static int do_dtovcfg(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[]) {
    ulong base_dtb_addr;
    const char *config_file_path = NULL;
    const char *devtype = NULL;
    const char *devpart = NULL;
    loff_t file_size;
    loff_t file_actread;
    char *config_file_data = NULL;
    loff_t dtbo_size;
    loff_t dtbo_actread; 
    char *dtbo_file_data = NULL;
    config_data_t config_data;
    int ret = 0;

    if (argc != 3) {
        return CMD_RET_USAGE;
    }

    base_dtb_addr = simple_strtoul(argv[1], NULL, 16);
    config_file_path = argv[2];

    devtype = env_get("devtype");
    devpart = env_get("devpart");

    if (!devtype || !devpart) {
        WUDBG("Error: devtype or devpart environment variable not set\n");
        goto out;
    }

    WUDBG("Base Device Tree Address: 0x%lx", base_dtb_addr);
    WUDBG("Config File Path: %s", config_file_path);
    WUDBG("Device Type: %s", devtype);
    WUDBG("Device Number: %s", devpart);

    ret = fs_set_blk_dev(devtype, devpart, FS_TYPE_ANY);
    if (ret) {
        WUDBG("Error: Unable to set block device (ret: %d)", ret);
        goto out;
    }

    ret = fs_size(config_file_path, &file_size);
    if (ret) {
        WUDBG("Error: Could not get file size for %s (ret: %d)", config_file_path, ret);
        goto out;
    }

    config_file_data = (char *)malloc(file_size + 1);
    if (!config_file_data) {
        WUDBG("Error: Failed to allocate memory");
        goto out;
    }

    ret = fs_set_blk_dev(devtype, devpart, FS_TYPE_ANY);
    if (ret) {
        WUDBG("Error: Unable to set block device (ret: %d)", ret);
        goto out;
    }

    ret = fs_read(config_file_path, (ulong)config_file_data, 0, file_size, &file_actread);
    if (ret < 0 || file_actread != file_size) {
        WUDBG("Error: Could not read file %s (ret: %d, file_actread: %d)", config_file_path, ret, (int)file_actread);
        goto out;
    }

    config_file_data[file_size] = '\0';

    parse_config(config_file_data, file_size, &config_data);

    for (int i = 0; i < config_data.setenv_count; i++) {
        WUDBG("setenv[%d]: name = %s, value = %s", i, config_data.setenv_params[i].name, config_data.setenv_params[i].value ? config_data.setenv_params[i].value : "NULL");

        if (config_data.setenv_params[i].value) {
            env_set(config_data.setenv_params[i].name, config_data.setenv_params[i].value);
        } else {
            env_set(config_data.setenv_params[i].name, NULL);  // 删除环境变量
        }
    }
    
    for (int i = 0; i < config_data.dt_overlay_count; i++) {
        WUDBG("dtoverlay[%d]: %s", i, config_data.dt_overlays[i].overlay_name);
        ret = fs_set_blk_dev(devtype, devpart, FS_TYPE_ANY);
        if (ret) {
            WUDBG("Error: Unable to set block device (ret: %d)", ret);
            continue;
        }

        ret = fs_size(config_data.dt_overlays[i].overlay_name, &dtbo_size);
        if (ret) {
            WUDBG("Error: Could not get file size for %s (ret: %d)", config_data.dt_overlays[i].overlay_name, ret);
            continue;
        }

        ret = fs_set_blk_dev(devtype, devpart, FS_TYPE_ANY);
        if (ret) {
            WUDBG("Error: Unable to set block device (ret: %d)", ret);
            continue;
        }

        ret = fdt_shrink_to_minimum((void *)base_dtb_addr, dtbo_size);
        if (ret < 0) {
            WUDBG("Error: fdt_shrink_to_minimum");
            continue;
        }

        dtbo_file_data = (char *)malloc(dtbo_size + 1);
        if (!dtbo_file_data) {
            WUDBG("Error: Failed to allocate memory");
            continue;
        }

        ret = fs_read(config_data.dt_overlays[i].overlay_name, (ulong)dtbo_file_data, 0, dtbo_size, &dtbo_actread);
        if (ret < 0 || dtbo_actread != dtbo_size) {
            WUDBG("Error: Could not read file %s (ret: %d, dtbo_size:%d dtbo_actread: %d)", config_data.dt_overlays[i].overlay_name, ret, (int)dtbo_size, (int)dtbo_actread);
            if (dtbo_file_data) {
                free(dtbo_file_data);
                dtbo_file_data = NULL;
            }
            continue;
        }

        WUDBG("dt_addr:0x%0lx  overlay_addr:0x%0lx", base_dtb_addr, (ulong)dtbo_file_data);
        fdt_overlay_apply_verbose((void *)base_dtb_addr, (void *)dtbo_file_data);
        if (ret < 0) {
            WUDBG("Error: Could not apply %s ", config_data.dt_overlays[i].overlay_name);
            if (dtbo_file_data) {
                free(dtbo_file_data);
                dtbo_file_data = NULL;
            }
            continue;   
        }

        if (dtbo_file_data) {
            free(dtbo_file_data);
            dtbo_file_data = NULL;
        }
    }

    for (int i = 0; i < config_data.set_param_count; i++) {
		int  nodeoffset;	/* node offset from libfdt */
        #define SCRATCHPAD	1024		/* bytes of scratchpad memory */
		static char data[SCRATCHPAD] __aligned(4);/* property storage */
		const void *ptmp;
		int  len;		/* new length of the property */
		int  ret;		/* return value */

        WUDBG("set[%d]: path = %s, prop = %s, value = %s", i, config_data.set_params[i].path, config_data.set_params[i].prop, config_data.set_params[i].value ? config_data.set_params[i].value : "NULL");
        ret = fdt_shrink_to_minimum((void *)base_dtb_addr, SCRATCHPAD);
        if (ret < 0) {
            WUDBG("Error: fdt_shrink_to_minimum");
            continue;
        }

		nodeoffset = fdt_path_offset ((const void *)base_dtb_addr, config_data.set_params[i].path);
		if (nodeoffset < 0) {
			/*
			 * Not found or something else bad happened.
			 */
			WUDBG ("libfdt fdt_path_offset(0x%0lx,%s) returned %s",base_dtb_addr,
                config_data.set_params[i].path,
				fdt_strerror(nodeoffset));
			continue;
		}

		if (!config_data.set_params[i].value) {
			len = 0;
		} else {
			ptmp = fdt_getprop((const void *)base_dtb_addr, nodeoffset, config_data.set_params[i].prop, &len);
			if (len > SCRATCHPAD) {
				WUDBG("prop (%d) doesn't fit in scratchpad!",len);
				continue;
			}
			if (ptmp != NULL)
				memcpy(data, ptmp, len);

			ret = fdt_parse_prop(config_data.set_params[i].value, data, &len);
			if (ret != 0)
				continue;
		}

		ret = fdt_setprop((void *)base_dtb_addr, nodeoffset, config_data.set_params[i].prop, data, len);
		if (ret < 0) {
			WUDBG ("libfdt fdt_setprop(): %s", fdt_strerror(ret));
			continue;
		}

    }

    for (int i = 0; i < config_data.node_count; i++) {
		int  nodeoffset;	/* node offset from libfdt */
		int  ret;

        WUDBG("mknode[%d]: path = %s, node = %s", i, config_data.nodes[i].path, config_data.nodes[i].node);
        ret = fdt_shrink_to_minimum((void *)base_dtb_addr, SCRATCHPAD);
        if (ret < 0) {
            WUDBG("Error: fdt_shrink_to_minimum");
            continue;
        }

		nodeoffset = fdt_path_offset ((void *)base_dtb_addr, config_data.nodes[i].path);
		if (nodeoffset < 0) {
			/*
			 * Not found or something else bad happened.
			 */
			WUDBG ("libfdt fdt_path_offset() returned %s",
				fdt_strerror(nodeoffset));
			continue;
		}

		ret = fdt_add_subnode((void *)base_dtb_addr, nodeoffset, config_data.nodes[i].node);
		if (ret < 0) {
			WUDBG ("libfdt fdt_add_subnode(): %s",
				fdt_strerror(ret));
			continue;
		}

    }

    for (int i = 0; i < config_data.rm_count; i++) {
	    int  nodeoffset;	/* node offset from libfdt */
		int  ret;

        WUDBG("rm[%d]: path = %s, prop = %s", i, config_data.rm_targets[i].path, config_data.rm_targets[i].prop ? config_data.rm_targets[i].prop : "NULL");
        ret = fdt_shrink_to_minimum((void *)base_dtb_addr, SCRATCHPAD);
        if (ret < 0){
            WUDBG("Error: fdt_shrink_to_minimum");
            continue;
        }
		/*
		 * Get the path.  The root node is an oddball, the offset
		 * is zero and has no name.
		 */
		nodeoffset = fdt_path_offset ((void *)base_dtb_addr, config_data.rm_targets[i].path);
		if (nodeoffset < 0) {
			/*
			 * Not found or something else bad happened.
			 */
			WUDBG ("libfdt fdt_path_offset() returned %s",
				fdt_strerror(nodeoffset));
			continue;
		}
		/*
		 * Do the delete.  A fourth parameter means delete a property,
		 * otherwise delete the node.
		 */
		if (config_data.rm_targets[i].prop) {
			ret = fdt_delprop((void *)base_dtb_addr, nodeoffset, config_data.rm_targets[i].prop);
			if (ret < 0) {
				WUDBG("libfdt fdt_delprop():  %s",
					fdt_strerror(ret));
				continue;
			}
		} else {
			ret = fdt_del_node((void *)base_dtb_addr, nodeoffset);
			if (ret < 0) {
				WUDBG("libfdt fdt_del_node():  %s",
					fdt_strerror(ret));
				continue;
			}
		}

    }

    ret = CMD_RET_SUCCESS;

out:
    if (config_file_data)
        free(config_file_data);

    if (dtbo_file_data)
        free(dtbo_file_data);

    return ret;
}

U_BOOT_CMD(
    dtovcfg, 3, 0, do_dtovcfg,
    "Parse config.txt file and apply device tree overlays",
    "<base_dtb_addr> <config_file_path>\n"
    " - <base_dtb_addr>: Address of the base Device Tree Blob (DTB) in memory.\n"
    " - <config_file_path>: Path to the configuration file (config.txt) that specifies the overlays."
);
